/*
 * @FileName: agv_teleop_keyboard.cpp
 * @Author: haibo.yang 
 * @Date: 2022-04-18 15:44:20 
 * @Last Modified by: mikey.zhaopeng
 * @Last Modified time: 2022-04-18 15:47:26
 */

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
 
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_S 0x73
#define KEYCODE_W 0x77
#define KEYCODE_U 0x75
#define KEYCODE_J 0x6A
#define KEYCODE_I 0x69
#define KEYCODE_K 0x6B
#define KEYCODE_SPACE 0x20

#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57
#define KEYCODE_Q_CAP 0x51
#define KEYCODE_E_CAP 0x45

class TeleopKeyboard
{
  private:
  double walk_vel, run_vel, yaw_rate, yaw_rate_run, rvel, yrate, vel_step, yaw_step;
  geometry_msgs::Twist cmd;
  std::vector< std::pair<uint16_t, bool> > key_;

  ros::NodeHandle n_;
  ros::Publisher vel_pub_;

  public:
  TeleopKeyboard()
  {
    cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
    vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  
    ros::NodeHandle n_private("~");
    n_private.param("walk_vel", walk_vel, 0.5);
    n_private.param("run_vel", run_vel, 1.0);
    n_private.param("vel_step", vel_step, 0.1);
    n_private.param("yaw_rate", yaw_rate, 0.2);
    n_private.param("yaw_run_rate", yaw_rate_run, 1.0);
    n_private.param("vel_step", yaw_step, 0.1);

    rvel = walk_vel;
    yrate = yaw_rate;
  }

  struct comp
  {
    /* data */
    comp(uint16_t const& n) : _n(n) {}
    bool operator() (std::pair<uint16_t, bool> const& p)
    {
      return (p.first == _n);
    }
    uint16_t _n;
  };
  
  ~TeleopKeyboard() { }
  void msgHandle();

  void stopRobot() 
  {
      cmd.linear.x = 0.0;
      cmd.angular.z = 0.0;
      vel_pub_.publish(cmd);
  }

};

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  exit(0);
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_keyboard", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
  ROS_INFO("teleop_keyboard Start");
  TeleopKeyboard tpk;
  signal(SIGINT,quit);

  boost::thread t = boost::thread(boost::bind(&TeleopKeyboard::msgHandle, &tpk));
  ros::spin();

  t.interrupt();
  t.join();
  tpk.stopRobot();
  // tcsetattr(kfd, TCSANOW, &cooked);

  return(0);
}

void TeleopKeyboard::msgHandle(void)
{
  char c; 
  bool dirty = false;
  bool on = true;
  //get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  struct pollfd ufd;
  ufd.fd = kfd;
  ufd.events = POLLIN;

  for (;;) {
    boost::this_thread::interruption_point();
    // get the next event from the keyboard
    int num;
    
    if ((num = poll(&ufd, 1, 250)) < 0) {
        perror("poll():");
        return;
    } else if(num > 0) {
        if(read(kfd, &c, 1) < 0) {
            perror("read():");
            return;
        } 
        // else {
        //   ROS_INFO("got key:%c %02x", c, c);
        // }
    } else {
        // if (dirty == true) {
        //     stopRobot();
        //     dirty = false;
        // }
        // printf("*\n");
        continue;
    }

    switch(c)
    {
    // Walking
    case KEYCODE_W:
      if (on)
        cmd.linear.x = rvel;
      else
        cmd.linear.x = 0;
      dirty = true;
      break;
    case KEYCODE_S:
      if (on)
        cmd.linear.x = - rvel;
      else
        cmd.linear.x = 0;
      dirty = true;
      break;
    case KEYCODE_A:
      if (on)
        cmd.angular.z = yrate;
      else
        cmd.angular.z = 0;
      dirty = true;
      break;
    case KEYCODE_D:
      if (on)
        cmd.angular.z = - yrate;
      else
        cmd.angular.z = 0;
      dirty = true;
      break;
    case KEYCODE_U:
      if (on) {
        if (rvel < run_vel)
          rvel += vel_step;
        if (rvel > run_vel)
          rvel = run_vel;
        ROS_INFO("line speed up :%.2f", rvel);
        if (fabs(cmd.linear.x) > 0) {
          cmd.linear.x = cmd.linear.x > 0 ? rvel : -rvel;
          dirty = true;
        }
      }
      break;
    case KEYCODE_J:
      if (on) {
        if (rvel > walk_vel)
          rvel -= vel_step;
        if (rvel < walk_vel)
          rvel = walk_vel;
        ROS_INFO("line speed down :%.2f", rvel);
        if (fabs(cmd.linear.x) > 0) {
          cmd.linear.x = cmd.linear.x > 0 ? rvel : -rvel;
          dirty = true;
        }
      }
      break;
    case KEYCODE_I:
      if (on) {
        if (yrate < yaw_rate_run)
          yrate += yaw_step;
        if (yrate > yaw_rate_run)
          yrate = yaw_rate_run;
        ROS_INFO("yaw speed up :%.2f", yrate);
        if (fabs(cmd.angular.z) > 0) {
          cmd.angular.z = cmd.angular.z > 0 ? yrate : -yrate;
          dirty = true;
        }
      }
      break;
    case KEYCODE_K:
      if (on) {
        if (yrate > yaw_rate)
          yrate -= yaw_step;
        if (yrate < yaw_rate)
          yrate = yaw_rate;
        ROS_INFO("yaw speed down :%.2f", yrate);
        if (fabs(cmd.angular.z) > 0) {
          cmd.angular.z = cmd.angular.z > 0 ? yrate : -yrate;
          dirty = true;
        }
      }
      break;
    case KEYCODE_SPACE:
      stopRobot();
      break;
    }

    if (dirty) {
      vel_pub_.publish(cmd);
      // std::vector< std::pair<uint16_t, bool> >::iterator i = std::find_if(key_.begin(), key_.end(), comp(key));
      // if (i != key_.end()) {
      //   if (!i->second)
      //     i->second = on;
      // } else {
      //   key_.push_back(std::make_pair(key, on));
      // }
    }
  }
}
